/***************************************************************************\
 * Copyright (C) by Keio University
 * KinectProcesssing.cu created in 06 2012.
 * Mail : fdesorbi@hvrl.ics.keio.ac.jp
 *
 * KinectProcesssing.cu is part of the HVRL Engine Library.
 *
 * The HVRL Engine Library is free software; you can redistribute it and/or modify
 * it under the terms of the GNU Lesser General Public License as published by
 * the Free Software Foundation; either version 3 of the License, or
 * (at your option) any later version.
 *
 * The HVRL Engine Library is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU Lesser General Public License for more details.
 *
 * You should have received a copy of the GNU Lesser General Public License
 * along with this program.  If not, see <http://www.gnu.org/licenses/>.
 *
 \***************************************************************************/

#ifndef KINECT_PROCESSING_CU_
#define KINECT_PROCESSING_CU_

#ifdef WIN32
#define NOMINMAX
#include <windows.h>
#endif

#include <hvrl/gpgpu/cuda/Prototypes.hpp>
#include <hvrl/gpgpu/cuda/Tools.cuh>

namespace hvrl {

namespace cuda {

namespace kinectprocessing {

__device__ __constant__ float d_intrinsic[3]; /**< Hold the intrinsic parameters of Kinect considering pixels are squared (kinect back projection coefficient, principal point)*/

__global__ void d_back_project(float *outputv, const short unsigned int *inputv,
		float *outputc, const unsigned char *inputc, float bpc, int w, int h) {

	int x = __umul24(blockIdx.x, blockDim.x) + threadIdx.x;
	int y = __umul24(blockIdx.y, blockDim.y) + threadIdx.y;

	int pos_inputv = y * w + x;
	int pos_outputv = pos_inputv * 4;
	int pos_inputc = pos_inputv * 3;
	int pos_outputc = pos_inputv * 4;
	float w2 = w/2.0f;
	float h2 = h/2.0f;

	if (x < w && x >= 0 && y < h && y >= 0) {
		if (inputv[pos_inputv] == 0) {
			outputv[pos_outputv + 3] = 0.0f;
			float z = 0.0f;
			outputv[pos_outputv] = (/*w - */x - w2) * z * bpc;
			outputv[pos_outputv + 1] = (h - y - h2) * z * bpc;
			outputv[pos_outputv + 2] = -z;

			outputc[pos_outputc] = inputc[pos_inputc] * 0.003921569f;
			outputc[pos_outputc + 1] = inputc[pos_inputc + 1] * 0.003921569f;
			outputc[pos_outputc + 2] = inputc[pos_inputc + 2] * 0.003921569f;
			outputc[pos_outputc + 3] = 0.0f;
		} else {
			outputv[pos_outputv + 3] = 1.0f;
			// Come from https://groups.google.com/forum/?fromgroups=#!topic/openkinect/AxNRhG_TPHg
			float z = inputv[pos_inputv]*0.001f;//0.1236f * tan(inputv[pos_inputv]/2842.5f + 1.1863f);*/0.00307110156374373*inputv[pos_inputv]-3.33094951605675;
			// Another possibility is http://www.ros.org/wiki/kinect_node/Calibration
			outputv[pos_outputv] = (/*w -*/ x - w2)* z * bpc;
			outputv[pos_outputv + 1] = (h - y - h2)* z * bpc;
			outputv[pos_outputv + 2] = -z;
			outputc[pos_outputc] = inputc[pos_inputc] * 0.003921569f;
			outputc[pos_outputc + 1] = inputc[pos_inputc + 1] * 0.003921569f;
			outputc[pos_outputc + 2] = inputc[pos_inputc + 2] * 0.003921569f;
			outputc[pos_outputc + 3] = 1.0f;
		}
	}

}

__global__ void d_back_project(float *outputv, const float *inputv,
		float *outputc, const unsigned char *inputc, const int w, const int h) {

	int x = __umul24(blockIdx.x, blockDim.x) + threadIdx.x;
	int y = __umul24(blockIdx.y, blockDim.y) + threadIdx.y;

	int pos_inputv = y * w + x;
	int pos_outputv = pos_inputv * 4;
	int pos_inputc = pos_inputv * 3;
	int pos_outputc = pos_inputv * 4;

	if (x < w && x >= 0 && y < h && y >= 0) {
		if (inputv[pos_inputv] < 100 || inputv[pos_inputv] > 8000) {
			outputv[pos_outputv + 2] = 10.0f;
			outputc[pos_outputc + 3] = 0.0f;
		} else {
			outputv[pos_outputv + 2] = inputv[pos_inputv] * 0.001f;
			outputc[pos_outputc + 3] = 1.0f;
		}
		outputv[pos_outputv + 3] = 1.0f;
		outputv[pos_outputv] = (x - d_intrinsic[1])
				* outputv[pos_outputv + 2] * d_intrinsic[0];
		outputv[pos_outputv + 1] = (y - d_intrinsic[2])
				* outputv[pos_outputv + 2] * d_intrinsic[0];
		outputc[pos_outputc] = inputc[pos_inputc] * 0.003921569f;
		outputc[pos_outputc + 1] = inputc[pos_inputc + 1] * 0.003921569f;
		outputc[pos_outputc + 2] = inputc[pos_inputc + 2] * 0.003921569f;


	}

}

__global__ void d_compute_normal(float *output, const float *inputv,
		const float *inputc, const int w, const int h) {

	int x = __umul24(blockIdx.x, blockDim.x) + threadIdx.x;
	int y = __umul24(blockIdx.y, blockDim.y) + threadIdx.y;

	int pos_inputv = (y * w + x) * 3;
	int pos_inputc = (y * w + x) * 4;
	int pos_output = pos_inputv;
	float3 p1, p2;
	bool exist = (inputc[pos_inputc + 3] != 0.0f);

	float3 p = make_float3(inputv[pos_inputv], inputv[pos_inputv + 1],
			inputv[pos_inputv + 2]);

	if (x < w && x >= 0 && y < h && y >= 0) {
		if(exist){
			if (x == w - 1) {
				p1 = make_float3(inputv[pos_inputv - 3], inputv[pos_inputv - 2],
						inputv[pos_inputv - 1]);
				exist &= (inputc[pos_inputc - 1] != 0.0f);
			} else {
				p1 = make_float3(inputv[pos_inputv + 3], inputv[pos_inputv + 4],
						inputv[pos_inputv + 5]);
				exist &= (inputc[pos_inputc + 7] != 0.0f);
			}

			if (y == h - 1) {
				p2 = make_float3(inputv[pos_inputv - w * 3],
						inputv[pos_inputv - w * 3 + 1],
						inputv[pos_inputv - w * 3 + 2]);
				exist &= (inputc[pos_inputc - w * 4 + 3] != 0.0f);
			} else {
				p2 = make_float3(inputv[pos_inputv + w * 3],
						inputv[pos_inputv + w * 3 + 1],
						inputv[pos_inputv + w * 3 + 2]);
				exist &= (inputc[pos_inputc + w * 4 + 3] != 0.0f);
			}
		}
		if (exist == false) {
			output[pos_output + 2] = 0.0f;
			output[pos_output + 1] = 0.0f;
			output[pos_output] = 0.0f;
		} else {
			float3 v1 = p1 - p;
			float3 v2 = p2 - p;
			float3 cross_p = cross(v1, v2);

			output[pos_output] = cross_p.x;
			output[pos_output + 2] = cross_p.y;
			output[pos_output + 1] = cross_p.z;
		}
	}

}

bool updateBackProjectionMatrix(const float& kinectbackprojection,
		const unsigned int& width, const unsigned int& height) {

	float tmp[3] = { kinectbackprojection, float(width) / 2.0f, float(height)
			/ 2.0f };
	return checkStatus(cudaMemcpyToSymbol(d_intrinsic, tmp, 3 * sizeof(float)));

}


void backProject(short unsigned int *d_input_vertex,
		unsigned char *d_input_color, float *d_output_vertex,
		float *d_output_color, float bpc, const int& d_width, const int& d_height) {
	// Define the size of the grid and the block
	dim3 blockSize(16,16);
	dim3 gridSize((d_width + blockSize.x - 1) / blockSize.x,
			(d_height + blockSize.y - 1) / blockSize.y);
d_back_project <<< gridSize, blockSize>>>(d_output_vertex, d_input_vertex, d_output_color, d_input_color, bpc, d_width, d_height);
}

void backProject(float *d_input_vertex, unsigned char *d_input_color,
		float *d_output_vertex, float *d_output_color, const int& d_width,
		const int& d_height) {
	// Define the size of the grid and the block
	dim3 blockSize(32, 32);
	dim3 gridSize((d_width + blockSize.x - 1) / blockSize.x,
			(d_height + blockSize.y - 1) / blockSize.y);

d_back_project <<< gridSize, blockSize>>>(d_output_vertex, d_input_vertex, d_output_color, d_input_color, d_width, d_height);

}

}

}
}

#endif
